--敏石项目,铝条下料
--@author: wxf
--@date: 2025-09-14

--node-red 下发给机器人的命令: 1~11 ，机器人开始执行时，反馈 1~11,
--机器人执行完毕后,将状态回传给 node-red : 101~111
--依赖 AI request , 和 AO response

--双臂初始化
local request_DoInit = 1
--左臂拍照测量纵深,赋给 offsetY
local request_LeftDoMesure = 2
--左臂根据 offsetX offsetY 去上端脱夹
local request_LeftDoUpDeClip = 3
--左臂根据 offsetX offsetY 去下端脱夹
local request_LeftDoDownDeClip = 4
--右臂抓住铝条
local request_RightDoHold = 5
--左臂上端松开夹爪后,右臂拉铝条
local request_RightDoPull = 6
--左臂下端松开夹爪后,右臂拉铝条
local request_RightDoPull2 = 7
--右臂放置铝条
local request_RightDoPut = 8
--全局相机拍照,确定目标铝条水平偏移量,赋值给 offsetX
local request_DoCenterMesure = 9
--左臂上端脱夹后,回到初始位置
local request_LeftUpDeClipToInit = 10
--左臂下端脱夹后,回到初始位置
local request_LeftDownDeClipToInit = 11

--左臂相机交互
local response_LeftDoMesure_call = request_LeftDoMesure + 1000
local request_LeftDoMesure_call = request_LeftDoMesure + 10000
--全局相机交互
local response_DoCenterMesure_call = request_DoCenterMesure + 1000
local request_DoCenterMesure_call = request_DoCenterMesure + 10000

local RightGripper = "DO30_1"  --右臂夹爪
local LeftGripper = "DO20_1" --左臂夹爪
local GripperOpen = 0
local GripperClose = 1



--左臂运动到位置
function DoInitLeft()
    MoveAbsJ("R_DL6",LeftInit, v200,fine, tool0, wobj0, load0)
end

--右臂运动到初始位置
function DoInitRight()
    MoveAbsJ("R_DR6",RightInit, v200,fine, right_tool, wobj0, load0)
end

--两臂运动到初始位置
function DoInit()
    MoveToInitLeft()
    MoveToInitRight()
end



--左臂上端脱夹,从初始化位置,运动到上端脱夹位置上部分 100毫米, 打开夹爪, 下来, 撑开夹爪
--脱夹位置,基于初始示教位置,水平方向增加偏移量
function LeftDoUpDeClip()
    SetDO(LeftGripper,GripperOpen)
    MoveAbsJ("R_DL6",LeftInit, v200,fine, tool0, wobj0, load0)
    MoveAbsJ("R_DL6",LeftDoUpDeClip1, v200,fine, tool0, wobj0, load0)
    MoveAbsJ("R_DL6",LeftDoUpDeClip2, v200,fine, tool0, wobj0, load0)

    local robTarget = CopyRobTarget( LeftDoUpDeClip3 )
    robTarget.trans.x = robTarget.trans.x + GetAI("offsetX")
    robTarget.trans.y = robTarget.trans.y + GetAI("offsetY")
    robTarget.trans.y = robTarget.trans.y + 150
    robTarget.trans.z = robTarget.trans.z - 50
    MoveL("R_DL6",robTarget, v200,fine, tool0, wobj0, load0)
    robTarget.trans.y = robTarget.trans.y - 150
    MoveL("R_DL6",robTarget, v200,fine, tool0, wobj0, load0)
    robTarget.trans.z = robTarget.trans.z + 50
    MoveL("R_DL6",robTarget, v200,fine, tool0, wobj0, load0)
    Sleep(1000)
    SetDO(LeftGripper,GripperClose)
end

--松开爪子,以当前姿态上升,回到初始位置
function LeftUpDeClipToInit()
    SetDO(LeftGripper,GripperOpen)
    Sleep(1000)
    local current_t = GetRobTarget ("R_DL6",tool0,wobj0)
    current_t.trans.z = current_t.trans.z - 50
    MoveL("R_DL6",current_t, v200,fine, tool0, wobj0, load0)
    current_t.trans.y = current_t.trans.y + 150
    MoveL("R_DL6",current_t, v200,fine, tool0, wobj0, load0)

    MoveAbsJ("R_DL6",LeftDoUpDeClip2, v200,fine, tool0, wobj0, load0)
    MoveAbsJ("R_DL6",LeftDoUpDeClip1, v200,fine, tool0, wobj0, load0)
    MoveAbsJ("R_DL6",LeftInit, v200,fine, tool0, wobj0, load0)
end





--右臂抓住铝条，基于偏移量 offsetX, offsetY 
function RightDoHold()
    SetDO(RightGripper,GripperOpen)
    MoveAbsJ("R_DR6",RightInit, v200,fine, right_tool, wobj0, load0)
    MoveAbsJ("R_DR6",RightDoHold1, v200,fine, right_tool, wobj0, load0)
    MoveAbsJ("R_DR6",RightDoHold2, v200,fine, right_tool, wobj0, load0)

    local robTarget = CopyRobTarget( PRightDoHold3 )
    robTarget.trans.x = robTarget.trans.x + GetAI("offsetX")
    robTarget.trans.y = robTarget.trans.y + GetAI("offsetY")
    robTarget.trans.x = robTarget.trans.x + 50
    MoveL("R_DR6",robTarget, v200,fine, right_tool, wobj0, load0)
    robTarget.trans.x = robTarget.trans.x - 50
    MoveL("R_DR6",robTarget, v200,fine, right_tool, wobj0, load0)
    Sleep(1000)
    SetDO(RightGripper,GripperClose)
    Sleep(1000)
end

function RightDoPull()
    local current_t = GetRobTarget ("R_DR6",right_tool,wobj0)
    current_t.trans.x = current_t.trans.x + 20
    MoveL("R_DR6",current_t, v200,fine, right_tool, wobj0, load0)
end

function RightDoPut()
    MoveAbsJ("R_DR6",RightDoPut1, v200,fine, right_tool, wobj0, load0)
    MoveAbsJ("R_DR6",RightDoPut2, v200,fine, right_tool, wobj0, load0)
    MoveAbsJ("R_DR6",RightDoPut3, v200,fine, right_tool, wobj0, load0)
    MoveL("R_DR6",RightDoPut4, v200,fine, right_tool, wobj0, load0)
    MoveL("R_DR6",RightDoPut5, v200,fine, right_tool, wobj0, load0) 
    SetDO(RightGripper,GripperOpen)
    Sleep(1000)
    MoveL("R_DR6",RightDoPut4, v200,fine, right_tool, wobj0, load0)
    MoveAbsJ("R_DR6",RightDoPut3, v200,fine, right_tool, wobj0, load0)
    MoveAbsJ("R_DR6",RightInit, v200,fine, right_tool, wobj0, load0)
    Sleep(1000)
end



function DoCenterMesure()
    MoveToInitLeft()
    MoveToInitRight()
    --相机执行拍照,如果交互失败,就重复等待,并增加人为启动干预
    SetAO("OP20_response", response_DoCenterMesure_call)

    Sleep(1000)
    flag = WaitAI("OP20_request","GT",request_DoCenterMesure_call-1,60000)
    while flag == 0 do
        TPWrite("request_DoCenterMesure_call timeout")
        Stop()
        flag = WaitAO("OP20_request","GT",request_DoCenterMesure_call-1,60000)
    end
    flag = GetAI("offsetX")
    while flag == 0 do
        TPWrite("offsetX 0 error")
        Stop()
        flag = GetAI("offsetX")
    end
    MoveAbsJ("R_DL6",LeftInit, v200,fine, tool0, wobj0, load0)
    MoveAbsJ("R_DR6",RightInit, v200,fine, right_tool, wobj0, load0)
end


function test()
    MoveAbsJ("R_DL6",LeftInit,v200,fine,tool0,wobj0,load0)
    MoveAbsJ("R_DR6",RightInit,v200,fine,right_tool,wobj0,load0)
    MoveAbsJ("R_DL6",LeftDoMesure1,v200,fine,tool0,wobj0,load0)
    MoveAbsJ("R_DL6",LeftDoMesure2,v200,fine,tool0,wobj0,load0)
    MoveAbsJ("R_DL6",LeftDoMesure3,v200,fine,tool0,wobj0,load0)
    MoveAbsJ("R_DL6",LeftDoUpDeClip1,v200,fine,tool0,wobj0,load0)
    MoveAbsJ("R_DL6",LeftDoUpDeClip2,v200,fine,tool0,wobj0,load0)
    MoveAbsJ("R_DL6",LeftDoDownDeClip1,v200,fine,tool0,wobj0,load0)
    MoveAbsJ("R_DL6",LeftDoDownDeClip2,v200,fine,tool0,wobj0,load0)
    MoveAbsJ("R_DR6",RightDoHold1,v200,fine,right_tool,wobj0,load0)
    MoveAbsJ("R_DR6",RightDoHold2,v200,fine,right_tool,wobj0,load0)
    MoveAbsJ("R_DR6",RightDoPut1,v200,fine,right_tool,wobj0,load0)
    MoveAbsJ("R_DR6",RightDoPut2,v200,fine,right_tool,wobj0,load0)
    MoveAbsJ("R_DR6",RightDoPut3,v200,fine,right_tool,wobj0,load0)

    MoveAbsJ("R_DL6",DoCenterMesureLeft,v200,fine,tool0,wobj0,load0)
    MoveAbsJ("R_DR6",DoCenterMesureRight,v200,fine,right_tool,wobj0,load0)

    MoveL("R_DL6",LeftDoUpDeClip3,v200,fine,tool0,wobj0,load0)
    MoveL("R_DL6",LeftDoDownDeClip3,v200,fine,tool0,wobj0,load0)
    MoveL("R_DR6",PRightDoHold3,v200,fine,right_tool,wobj0,load0)
    MoveL("R_DR6",RightDoPut4,v200,fine,right_tool,wobj0,load0)
    MoveL("R_DR6",RightDoPut5,v200,fine,right_tool,wobj0,load0)
end

SetAO("OP20_response", 0)
while 2>1 do
    Sleep(100)
    local request = GetAI("OP20_request")
    if request == request_DoInit then
        SetAO("OP20_response", request)
        DoInit()
        SetAO("OP20_response", request + 100)

    elseif request == request_LeftDoUpDeClip then
        SetAO("OP20_response", request)
        LeftDoUpDeClip()
        SetAO("OP20_response", request + 100)

    elseif request == request_LeftUpDeClipToInit then
        SetAO("OP20_response", request)
        LeftUpDeClipToInit()
        SetAO("OP20_response", request + 100)

    elseif request == request_RightDoHold then
        SetAO("OP20_response", request)
        RightDoHold()
        SetAO("OP20_response", request + 100)
    elseif request == request_RightDoPull then
        SetAO("OP20_response", request)
        RightDoPull()
        SetAO("OP20_response", request + 100)  
    elseif request == request_RightDoPut then
        SetAO("OP20_response", request)
        RightDoPut()
        SetAO("OP20_response", request + 100)
    elseif request == request_DoCenterMesure then
        SetAO("OP20_response", request)
        DoCenterMesure()
        SetAO("OP20_response", request + 100)
    end
end

local function GLOBALDATA_DEFINE()
JOINTTARGET("DoCenterMesureLeft",{26.272,24.788,69.682,7.491,77.898,5.258,0.000},{0.000,0.000,0.000,0.000,0.000,0.000,0.000})
JOINTTARGET("DoCenterMesureRight",{26.272,24.788,69.682,7.491,77.898,5.258,0.000},{0.000,0.000,0.000,0.000,0.000,0.000,0.000})
JOINTTARGET("K10",{-5.684,23.205,-107.344,4.594,21.149,1.958,0.000},{243.749,0.000,0.000,0.000,0.000,0.000,0.000})
JOINTTARGET("K20",{-7.675,16.311,-121.987,1.086,13.454,2.041,0.000},{266.993,0.000,0.000,0.000,0.000,0.000,0.000})
JOINTTARGET("K30",{15.179,95.466,46.198,41.377,101.853,1.098,0.000},{0.000,0.000,0.000,0.000,0.000,0.000,0.000})
JOINTTARGET("LeftDoDownDeClip1",{26.272,24.788,69.682,7.491,77.898,5.258,0.000},{0.000,0.000,0.000,0.000,0.000,0.000,0.000})
JOINTTARGET("LeftDoDownDeClip2",{26.272,24.788,69.682,7.491,77.898,5.258,0.000},{0.000,0.000,0.000,0.000,0.000,0.000,0.000})
JOINTTARGET("LeftDoMesure1",{26.272,24.788,69.682,7.491,77.898,5.258,0.000},{0.000,0.000,0.000,0.000,0.000,0.000,0.000})
JOINTTARGET("LeftDoMesure2",{26.272,24.788,69.682,7.491,77.898,5.258,0.000},{0.000,0.000,0.000,0.000,0.000,0.000,0.000})
JOINTTARGET("LeftDoMesure3",{26.272,24.788,69.682,7.491,77.898,5.258,0.000},{0.000,0.000,0.000,0.000,0.000,0.000,0.000})
JOINTTARGET("LeftDoUpDeClip1",{-18.322,-29.988,-57.993,-87.882,175.940,-95.293,0.000},{796.537,0.000,0.000,0.000,0.000,0.000,0.000})
JOINTTARGET("LeftDoUpDeClip2",{-17.902,-54.257,-57.353,-66.524,169.196,-94.053,0.000},{796.544,0.000,0.000,0.000,0.000,0.000,0.000})
JOINTTARGET("LeftInit",{-39.097,-29.837,-91.605,-74.864,144.718,-74.589,0.000},{796.547,0.000,0.000,0.000,0.000,0.000,0.000})
JOINTTARGET("RightDoHold1",{-34.514,65.176,107.887,12.139,55.519,-0.976,0.000},{0.000,0.000,0.000,0.000,0.000,0.000,0.000})
JOINTTARGET("RightDoHold2",{-26.864,88.555,96.034,0.215,63.142,-0.203,0.000},{0.000,0.000,0.000,0.000,0.000,0.000,0.000})
JOINTTARGET("RightDoPut1",{-16.450,88.387,92.250,3.838,73.517,0.702,0.000},{0.000,0.000,0.000,0.000,0.000,0.000,0.000})
JOINTTARGET("RightDoPut2",{-2.367,27.968,130.873,25.420,87.542,1.790,0.000},{0.000,0.000,0.000,0.000,0.000,0.000,0.000})
JOINTTARGET("RightDoPut3",{24.310,23.114,99.698,77.953,167.827,-71.269,0.000},{0.000,0.000,0.000,0.000,0.000,0.000,0.000})
JOINTTARGET("RightInit",{-58.373,60.399,133.675,-5.923,31.812,-4.967,0.000},{0.000,0.000,0.000,0.000,0.000,0.000,0.000})
ROBTARGET("LeftDoDownDeClip3",{1029.410,-0.060,381.530},{0.000366,-0.999970,-0.003234,0.006986},{-1,-1,0,1},{0.000,0.000,0.000,0.000,0.000,0.000,0.000},0.000)
ROBTARGET("LeftDoUpDeClip3",{-666.530,184.550,-196.800},{0.739301,-0.010489,-0.016267,0.673097},{-273396,-65536,-1,-131071},{796.547,0.000,0.000,0.000,0.000,0.000,0.000},0.000)
ROBTARGET("PRightDoHold3",{-854.340,67.410,52.610},{0.000621,0.050922,0.998696,-0.003505},{-343000,65536,0,0},{0.000,0.000,0.000,0.000,0.000,0.000,0.000},0.000)
ROBTARGET("RightDoPut4",{-649.050,-274.260,549.380},{0.454223,-0.517225,-0.492905,-0.532170},{-691621,0,1,-131071},{0.000,0.000,0.000,0.000,0.000,0.000,0.000},0.000)
ROBTARGET("RightDoPut5",{-458.000,272.270,649.170},{0.454218,-0.517238,-0.492903,-0.532163},{-1138321,65536,0,-131071},{0.000,0.000,0.000,0.000,0.000,0.000,0.000},0.000)
end
print("The end!")